#include "tf_calibration.h"

#include <string.h>

#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>

using namespace std;

// GUI STUFF
string windowname;
int objectID = -1;

cv::Rect button(0,150,500,250);
cv::Rect color0Rect(0, 0, 500/3, 150);
cv::Rect color1Rect(500/3, 0, 500/3, 150);
cv::Rect color2Rect(1000/3, 0, 500/3, 150);

// Publisher for debug images
ros::Publisher image_pub_detected;

// The calibrator
TfCalibration* calibrator = NULL;

/******************************************************************
Callback function for drawing the system interface to the window
****************************************************************/
void drawCallback(const ros::TimerEvent&)
{
	cv::Mat image(250, 500, CV_8UC3);
	image = cv::Scalar(0,0,0);

	cv::rectangle(image, color0Rect, cv::Scalar(calibrator->colors[0].colorHue,
												calibrator->colors[0].colorSat,255), -1);
	cv::rectangle(image, color1Rect, cv::Scalar(calibrator->colors[1].colorHue,
												calibrator->colors[1].colorSat,255), -1);
	cv::rectangle(image, color2Rect, cv::Scalar(calibrator->colors[2].colorHue,
												calibrator->colors[2].colorSat,255), -1);

	cv::cvtColor(image, image, CV_HSV2BGR);

	cv::rectangle(image, button, cv::Scalar(255,0,255), -1);
	cv::imshow(windowname, image);
	cv::waitKey(1);
}

// From camera to Frame
tf::TransformBroadcaster *broadcaster = NULL;

// From Frame to robot
tf::TransformBroadcaster *staticBroadcaster = NULL;
/******************************************************************
Callback function for broadcasting tf transformation between the kinect
camera coordinate system and the robot gripper coordinate system.
****************************************************************/
void tfBroadcastCallback(const ros::TimerEvent)
{
	tf::Quaternion orientation = tf::createQuaternionFromRPY(0, toRad(30), toRad(200));
	tf::Vector3 translation(700, 400, 450);

//	broadcaster->sendTransform(calibrator->RequestTransform());
	broadcaster->sendTransform( tf::StampedTransform(tf::Transform(orientation, translation),
									ros::Time::now()+ros::Duration(0.05),"frame_link","camera_link"));

	orientation = tf::createQuaternionFromRPY(0, 0, 0);
	translation = tf::Vector3(700, 0, 150);

	staticBroadcaster->sendTransform( tf::StampedTransform(tf::Transform(orientation, translation),
									ros::Time::now()+ros::Duration(0.05), "base_link","frame_link"));
}


void imageCallback(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &camInfo)
{
	cv_bridge::CvImagePtr cv_ptr;
	/**************************************************************************
  * Convert image to openCV format using the cv_bridge function.
  * If the operation fails give an error message.
  **************************************************************************/
	try
	{
		cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
	}
	catch(cv_bridge::Exception& e)
	{
		ROS_ERROR("Something went wrong when converting!");
		return;
	}

	// Run the calibration point detection
	calibrator->ImageCallback(cv_ptr->image, camInfo);

	/**************************************************
  * Publish debug images
  **************************************************/
	image_pub_detected.publish(cv_ptr->toImageMsg());
}



// Mouse callback to capture button clicks and similar
void mouse(int event, int x, int y, int, void* )
{
	cv::Point2d mouse(x,y);
	// Check if the left mouse button has been pushed
	if(event == CV_EVENT_LBUTTONDOWN)
	{
		// Check if the cursor is within the button
		if(button.contains(mouse))
			// Calculate the transformation
			calibrator->CalculateTransform();
	}
}


int main(int argc, char ** argv)
{
	/**
 * The ros::init() function needs to see argc and argv so that it can perform
 * any ROS arguments and name remapping that were provided at the command line. For programmatic
 * remappings you can use a different version of init() which takes remappings
 * directly, but for most command-line programs, passing argc and argv is the easiest
 * way to do it.  The third argument to init() is the name of the node.
 *
 * You must call one of the versions of ros::init() before using any other
 * part of the ROS system.
 */
	ros::init(argc, argv, "tf_calibrate");


	/**
 * NodeHandle is the main access point to communications with the ROS system.
 * The first NodeHandle constructed will fully initialize this node, and the last
 * NodeHandle destructed will close down the node.
 */
	ros::NodeHandle n;

	broadcaster = new tf::TransformBroadcaster;
	staticBroadcaster = new tf::TransformBroadcaster;
	calibrator = new TfCalibration(n);


	// Set up GUI
	cv::namedWindow(windowname);
	cv::createTrackbar("Color 0: Hue", windowname, &(calibrator->colors[0].colorHue), 180);
	cv::createTrackbar("Color 0: Span", windowname, &(calibrator->colors[0].colorSpan), 100);
	cv::createTrackbar("Color 0: Sat", windowname, &(calibrator->colors[0].colorSat), 255);

	cv::createTrackbar("Color 1: Hue", windowname, &(calibrator->colors[1].colorHue), 180);
	cv::createTrackbar("Color 1: Span", windowname, &(calibrator->colors[1].colorSpan), 100);
	cv::createTrackbar("Color 1: Sat", windowname, &(calibrator->colors[1].colorSat), 255);

	cv::createTrackbar("Color 2: Hue", windowname, &(calibrator->colors[2].colorHue), 180);
	cv::createTrackbar("Color 2: Span", windowname, &(calibrator->colors[2].colorSpan), 100);
	cv::createTrackbar("Color 2: Sat", windowname, &(calibrator->colors[2].colorSat), 255);

	cv::setMouseCallback(windowname, mouse);

	// Subscribe to image feed
	image_transport::ImageTransport imageTransport = image_transport::ImageTransport(n);
	image_transport::CameraSubscriber camMagic = imageTransport.subscribeCamera("camera/rgb/image_rect_color", 1, imageCallback);

	//Create publishers
	image_pub_detected = n.advertise<sensor_msgs::Image>("irb_picker/tracker/calibration", 1);

	// Create a periodic callback that will perform drawing (to display information previously gathered)
	ros::Timer drawTimer = n.createTimer(ros::Duration(0.1), drawCallback);
	ros::Timer tfTimer = n.createTimer(ros::Duration(0.1), tfBroadcastCallback);

	/**
 * ros::spin() will enter a loop, pumping callbacks.  With this version, all
 * callbacks will be called from within this thread (the main one).  ros::spin()
 * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
 */
	ros::spin();

	return 0;
}
